/**************************************************************************
 * @file usr_type3_compensator.c
 * @author 黑马小乌龟 (532352406@qq.com)
 * @brief 
 * @version 0.1
 * @date 2025-11-05
 * 
 * @copyright Copyright (c) 2025
 * 
 *************************************************************************/
#include "usr_type3_compensator.h"

// 全局补偿器实例
Type3Compensator comp;


void CalculateType3Coefficients(Type3Compensator* comp, 
                               float fz1, float fz2, float fz3, 
                               float fp1, float K, float Ts)
{
    float wz1 = 2.0f * PI * fz1;
    float wz2 = 2.0f * PI * fz2;
    float wz3 = 2.0f * PI * fz3;
    float wp1 = 2.0f * PI * fp1;
    
    // 计算中间变量
    float az1 = 2.0f/Ts + wz1, bz1 = -2.0f/Ts + wz1;
    float az2 = 2.0f/Ts + wz2, bz2 = -2.0f/Ts + wz2;
    float az3 = 2.0f/Ts + wz3, bz3 = -2.0f/Ts + wz3;
    float ap1 = 2.0f/Ts + wp1, bp1 = -2.0f/Ts + wp1;
    
    // 计算分子系数
    float c0 = az1 * az2;
    float c1 = az1 * bz2 + bz1 * az2;
    float c2 = bz1 * bz2;
    
    comp->b0 = K * c0 * az3;
    comp->b1 = K * (c0 * bz3 + c1 * az3);
    comp->b2 = K * (c1 * bz3 + c2 * az3);
    comp->b3 = K * c2 * bz3;
    
    // 计算分母系数
    float d0 = (4.0f/(Ts*Ts)) * ap1;
    float d1 = (4.0f/(Ts*Ts)) * (bp1 - 2.0f * ap1);
    float d2 = (4.0f/(Ts*Ts)) * (ap1 - 2.0f * bp1);
    float d3 = (4.0f/(Ts*Ts)) * bp1;
    
    // 首一化
    comp->b0 /= d0;
    comp->b1 /= d0;
    comp->b2 /= d0;
    comp->b3 /= d0;
    comp->a1 = d1 / d0;
    comp->a2 = d2 / d0;
    comp->a3 = d3 / d0;
    
    // 初始化历史状态
    comp->x1 = comp->x2 = comp->x3 = 0.0f;
    comp->y1 = comp->y2 = comp->y3 = 0.0f;
    
    // 设置饱和限制
    comp->saturation_high = 0.95f;  // 95%占空比上限
    comp->saturation_low = 0.05f;   // 5%占空比下限
}

float ExecuteType3Compensator(Type3Compensator* comp, float input)
{
    // 计算输出: y[n] = b0*x[n] + b1*x[n-1] + b2*x[n-2] + b3*x[n-3] 
    //                - a1*y[n-1] - a2*y[n-2] - a3*y[n-3]
    comp->output = comp->b0 * input 
                 + comp->b1 * comp->x1 
                 + comp->b2 * comp->x2 
                 + comp->b3 * comp->x3 
                 - comp->a1 * comp->y1 
                 - comp->a2 * comp->y2 
                 - comp->a3 * comp->y3;
    
    // 更新历史状态
    comp->x3 = comp->x2;
    comp->x2 = comp->x1;
    comp->x1 = input;
    
    comp->y3 = comp->y2;
    comp->y2 = comp->y1;
    comp->y1 = comp->output;
    
    // 应用饱和限制
    if (comp->output > comp->saturation_high)
        comp->output = comp->saturation_high;
    else if (comp->output < comp->saturation_low)
        comp->output = comp->saturation_low;
    
    return comp->output;
}


int32_t ExecuteType3Compensator_Q15(Type3Compensator_Q15* comp, int16_t input)
{
    int64_t output;  // 64位中间结果防止溢出
    
    // 定点计算
    output = (int64_t)comp->b0 * input 
           + (int64_t)comp->b1 * comp->x1 
           + (int64_t)comp->b2 * comp->x2 
           + (int64_t)comp->b3 * comp->x3 
           - (int64_t)comp->a1 * comp->y1 
           - (int64_t)comp->a2 * comp->y2 
           - (int64_t)comp->a3 * comp->y3;
    
    // 右移Q15位并转换为32位
    int32_t result = (int32_t)(output >> Q15_SHIFT);
    
    // 更新历史状态
    comp->x3 = comp->x2;
    comp->x2 = comp->x1;
    comp->x1 = input;
    
    comp->y3 = comp->y2;
    comp->y2 = comp->y1;
    comp->y1 = result;
    
    // 饱和处理
    if (result > comp->saturation_high)
        result = comp->saturation_high;
    else if (result < comp->saturation_low)
        result = comp->saturation_low;
    
    return result;
}